#!/usr/bin/env python3
import rospy
from std_msgs.msg import Float64MultiArray, String, Int32
import numpy as np
import argparse
import rospkg
import os

def main():

    rospy.init_node('teach_move_commander', anonymous=True)

    pub = rospy.Publisher('/command_move_joint', Float64MultiArray, queue_size=10)
    pub_drag = rospy.Publisher('/command_teach_move', Int32, queue_size=10)

    rospy.sleep(1)

    msg = Float64MultiArray()
    msg.data = [0.0, -0.78, 0.0, -1.5, 0.0, 1.0, 1.5708]
    pub.publish(msg)
    rospy.sleep(5)


    rospy.sleep(1)
    msg = Int32()
    print("drag move...")
    msg.data = 1
    pub_drag.publish(msg)

if __name__ == '__main__':
    main()
